{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import torch.utils.data as data\n",
    "\n",
    "import random\n",
    "import numbers\n",
    "from PIL import Image, ImageMath\n",
    "import os\n",
    "import os.path\n",
    "import numpy as np\n",
    "import struct\n",
    "import math\n",
    "\n",
    "import torch\n",
    "import torchvision\n",
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "import matplotlib.cm as cm\n",
    "\n",
    "import faiss\n",
    "import time\n",
    "\n",
    "import pcl\n",
    "from PIL import Image, ImageDraw\n",
    "import faiss\n",
    "\n",
    "import timeit\n",
    "\n",
    "%matplotlib qt5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class KNNBuilder_GPU:\n",
    "    def __init__(self, k):\n",
    "        self.k = k\n",
    "        self.dimension = 3\n",
    "        \n",
    "        # we need only a StandardGpuResources per GPU\n",
    "        self.res = faiss.StandardGpuResources()\n",
    "        self.res.setTempMemoryFraction(0.1)\n",
    "        self.flat_config = faiss.GpuIndexFlatConfig()\n",
    "        self.flat_config.device = 0\n",
    "        \n",
    "    def build_nn_index(self, database):\n",
    "        '''\n",
    "        :param database: numpy array of Nx3\n",
    "        :return: Faiss index, in CPU\n",
    "        '''\n",
    "#         index = faiss.GpuIndexFlatL2(self.res, self.dimension, self.flat_config)  # dimension is 3\n",
    "        index_cpu = faiss.IndexFlatL2(self.dimension)\n",
    "        index = faiss.index_cpu_to_gpu(self.res, int(0), index_cpu)\n",
    "        \n",
    "        index.add(database)\n",
    "        return index\n",
    "    \n",
    "    def search_nn(self, index, query, k):\n",
    "        '''\n",
    "        :param index: Faiss index\n",
    "        :param query: numpy array of Nx3\n",
    "        :return: D: numpy array of Nxk\n",
    "                 I: numpy array of Nxk\n",
    "        '''\n",
    "        D, I = index.search(query, k)\n",
    "        return D, I\n",
    "    \n",
    "    def self_build_search(self, x):\n",
    "        '''\n",
    "\n",
    "        :param x: numpy array of Nxd\n",
    "        :return: D: numpy array of Nxk\n",
    "                 I: numpy array of Nxk\n",
    "        '''\n",
    "        x = np.ascontiguousarray(x, dtype=np.float32)\n",
    "        index = self.build_nn_index(x)\n",
    "        D, I = self.search_nn(index, x, self.k)\n",
    "        return D, I\n",
    "    \n",
    "\n",
    "class KNNBuilder:\n",
    "    def __init__(self, k):\n",
    "        self.k = k\n",
    "        self.dimension = 3\n",
    "\n",
    "    def build_nn_index(self, database):\n",
    "        '''\n",
    "        :param database: numpy array of Nx3\n",
    "        :return: Faiss index, in CPU\n",
    "        '''\n",
    "        index = faiss.IndexFlatL2(self.dimension)  # dimension is 3\n",
    "        index.add(database)\n",
    "        return index\n",
    "\n",
    "    def search_nn(self, index, query, k):\n",
    "        '''\n",
    "        :param index: Faiss index\n",
    "        :param query: numpy array of Nx3\n",
    "        :return: D: numpy array of Nxk\n",
    "                 I: numpy array of Nxk\n",
    "        '''\n",
    "        D, I = index.search(query, k)\n",
    "        return D, I\n",
    "\n",
    "    def self_build_search(self, x):\n",
    "        '''\n",
    "\n",
    "        :param x: numpy array of Nxd\n",
    "        :return: D: numpy array of Nxk\n",
    "                 I: numpy array of Nxk\n",
    "        '''\n",
    "        x = np.ascontiguousarray(x, dtype=np.float32)\n",
    "        index = self.build_nn_index(x)\n",
    "        D, I = self.search_nn(index, x, self.k)\n",
    "        return D, I\n",
    "\n",
    "\n",
    "class FarthestSampler:\n",
    "    def __init__(self):\n",
    "        pass\n",
    "\n",
    "    def calc_distances(self, p0, points):\n",
    "        return ((p0 - points) ** 2).sum(axis=1)\n",
    "\n",
    "    def sample(self, pts, k):\n",
    "        farthest_pts = np.zeros((k, 3))\n",
    "        farthest_pts[0] = pts[np.random.randint(len(pts))]\n",
    "        distances = self.calc_distances(farthest_pts[0], pts)\n",
    "        for i in range(1, k):\n",
    "            farthest_pts[i] = pts[np.argmax(distances)]\n",
    "            distances = np.minimum(distances, self.calc_distances(farthest_pts[i], pts))\n",
    "        return farthest_pts\n",
    "\n",
    "    \n",
    "\n",
    "def axisEqual3D(ax):\n",
    "    extents = np.array([getattr(ax, 'get_{}lim'.format(dim))() for dim in 'xyz'])\n",
    "    sz = extents[:,1] - extents[:,0]\n",
    "    centers = np.mean(extents, axis=1)\n",
    "    maxsize = max(abs(sz))\n",
    "    r = maxsize/2\n",
    "    for ctr, dim in zip(centers, 'xyz'):\n",
    "        getattr(ax, 'set_{}lim'.format(dim))(ctr - r, ctr + r)\n",
    "\n",
    "\n",
    "def plot_pc(pc_np, z_cutoff=70, birds_view=False, color='height', size=0.3, ax=None):\n",
    "    # remove large z points\n",
    "    valid_index = pc_np[:, 2] < z_cutoff\n",
    "    pc_np = pc_np[valid_index, :]\n",
    "\n",
    "    if ax is None:\n",
    "        fig = plt.figure(figsize=(9, 9))\n",
    "        ax = Axes3D(fig)\n",
    "    if color == 'height':\n",
    "        c = pc_np[:,1]\n",
    "        ax.scatter(pc_np[:, 0].tolist(), pc_np[:, 1].tolist(), pc_np[:, 2].tolist(), s=size, c=c, cmap=cm.jet_r)\n",
    "    elif color == 'reflectance':\n",
    "        assert False\n",
    "    else:\n",
    "        ax.scatter(pc_np[:, 0].tolist(), pc_np[:, 1].tolist(), pc_np[:, 2].tolist(), s=size, c=color)\n",
    "\n",
    "    axisEqual3D(ax)\n",
    "    if True == birds_view:\n",
    "        ax.view_init(elev=0, azim=-90)\n",
    "    else:\n",
    "        ax.view_init(elev=-45, azim=-90)\n",
    "    # ax.invert_yaxis()\n",
    "    \n",
    "    return ax"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def load_pc_and_pose_kitti(seq, no):\n",
    "    '''\n",
    "    return: pc_np: Nx4 - x, y, z, reflectance\n",
    "    return: pose_np: 4x4\n",
    "    '''\n",
    "    pc_np = np.load('/ssd/dataset/odometry/data_odometry_velodyne/numpy/%02d/np_0.20_20480_r50/%06d.npy' % (seq, no))\n",
    "    pose_np = np.load('/ssd/dataset/odometry/poses/%02d/%06d.npz' % (seq, no))['pose']\n",
    "    return pc_np, pose_np\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "def transform_pc(pc, pose, pose_ref):\n",
    "    '''\n",
    "    transform pc to the reference frame\n",
    "    '''\n",
    "    pc_coord = pc[:, 0:3]  # Nx3\n",
    "    pc_coord_homo = np.concatenate((pc_coord, np.ones((pc_coord.shape[0], 1))), axis=1)  # Nx4\n",
    "    pc_coord_homo_transposed = np.transpose(pc_coord_homo)  # 4xN\n",
    "\n",
    "    pc_coord_ref_transposed = np.dot(np.dot(np.linalg.inv(pose_ref), pose), pc_coord_homo_transposed)  # 4xN\n",
    "    pc_coord_ref = np.transpose(pc_coord_ref_transposed)  # Nx4\n",
    "    \n",
    "    if pc.shape[1] == 3:\n",
    "        pc_ref = pc_coord_ref[:, 0:3]\n",
    "    else:\n",
    "        pc_ref = np.concatenate((pc_coord_ref[:, 0:3], pc[:, 3:]), axis=1)\n",
    "        \n",
    "    return pc_ref"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# evaluate the accuracy of the ground truth pose\n",
    "pc_0, pose_0 = load_pc_and_pose_kitti(0, 0)\n",
    "pc_1, pose_1 = load_pc_and_pose_kitti(0, 3)\n",
    "\n",
    "pc_0_at_1 = transform_pc(pc_0, pose_0, pose_1)\n",
    "\n",
    "print(pc_0.shape)\n",
    "print(pc_0_at_1.shape)\n",
    "\n",
    "fig = plt.figure(figsize=(9, 9))\n",
    "ax = Axes3D(fig)\n",
    "ax = plot_pc(pc_1, color=[0, 0, 1], ax=ax)\n",
    "ax = plot_pc(pc_0_at_1, color=[1, 0, 0], ax=ax)\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "def make_dataset_kitti(root):\n",
    "    seq_list = list(range(11))\n",
    "\n",
    "    # filter or not\n",
    "    np_folder = 'np_0.20_20480_r50_filtered_sn'\n",
    "        \n",
    "    accumulated_sample_num = 0\n",
    "    sample_num_list = []\n",
    "    accumulated_sample_num_list = []\n",
    "    folder_list = []\n",
    "    for seq in seq_list:\n",
    "        folder = os.path.join(root, 'data_odometry_velodyne', 'numpy', '%02d'%seq, np_folder)\n",
    "        folder_list.append(folder)\n",
    "        \n",
    "        sample_num = round(len(os.listdir(folder)))\n",
    "        accumulated_sample_num += sample_num\n",
    "        sample_num_list.append(sample_num)\n",
    "        accumulated_sample_num_list.append(round(accumulated_sample_num))\n",
    "        \n",
    "    return seq_list, folder_list, sample_num_list, accumulated_sample_num_list\n",
    "\n",
    "\n",
    "# statistics of Farthest Point Sampling, KITTI\n",
    "sampler = FarthestSampler()\n",
    "radius_cutoff = 50\n",
    "k = 256\n",
    "\n",
    "seq_list, folder_list, sample_num_list, accumulated_sample_num_list = make_dataset_kitti('/ssd/dataset/odometry')\n",
    "mean_list = []\n",
    "min_list = []\n",
    "max_list = []\n",
    "for i in range(0, 20000, 100):\n",
    "\n",
    "    for idx, accumulated_sample_num in enumerate(accumulated_sample_num_list):\n",
    "        if i < accumulated_sample_num:\n",
    "            break\n",
    "    folder = folder_list[idx]\n",
    "    seq = seq_list[idx]\n",
    "\n",
    "    if idx == 0:\n",
    "        index_in_seq = i\n",
    "    else:\n",
    "        index_in_seq = i - accumulated_sample_num_list[idx-1]\n",
    "    pc_np_file = os.path.join(folder, '%06d.npy' % index_in_seq)\n",
    "    pc = np.load(pc_np_file)\n",
    "    \n",
    "\n",
    "    # point cloud z cutoff\n",
    "    valid_index = np.linalg.norm(pc[:, [0, 2]], axis=1) < radius_cutoff\n",
    "    pc = pc[valid_index, :]\n",
    "\n",
    "    # point cloud random sampling\n",
    "    pc = pc[np.random.choice(pc.shape[0], 16384, replace=False), :]\n",
    "\n",
    "    farthest_pts = sampler.sample(pc[:, 0:3], k)\n",
    "\n",
    "    # visualization\n",
    "#     fig = plt.figure(figsize=(9, 9))\n",
    "#     ax = Axes3D(fig)\n",
    "#     ax = plot_pc(pc, color=[0, 0, 1], ax=ax)\n",
    "#     ax = plot_pc(farthest_pts, color=[1, 0, 0], size=3, ax=ax)\n",
    "#     plt.show()\n",
    "\n",
    "    # nearest neighbor analysis for point cloud\n",
    "#     knn_k=2\n",
    "#     knn = KNNBuilder(k=knn_k)\n",
    "\n",
    "#     index = knn.build_nn_index(np.ascontiguousarray(pc[:, 0:3], dtype=np.float32))\n",
    "#     D, I = knn.search_nn(index, np.ascontiguousarray(pc[:, 0:3], dtype=np.float32), k=knn_k)\n",
    "\n",
    "#     D = np.sqrt(np.fabs(D))\n",
    "#     D = D[:, knn_k-1:knn_k]\n",
    "#     print(D.shape)\n",
    "#     print('pc k=%d - mean %f, max %f, min %f' % (knn_k, np.mean(D), np.max(D), np.min(D)))\n",
    "    \n",
    "    \n",
    "    # nearest neighbor analysis for nodes\n",
    "    knn_k=2\n",
    "    knn = KNNBuilder(k=knn_k)\n",
    "\n",
    "    index = knn.build_nn_index(np.ascontiguousarray(farthest_pts[:, 0:3], dtype=np.float32))\n",
    "    D, I = knn.search_nn(index, np.ascontiguousarray(farthest_pts[:, 0:3], dtype=np.float32), k=knn_k)\n",
    "\n",
    "    D = np.sqrt(np.fabs(D))\n",
    "    D = D[:, knn_k-1:knn_k]\n",
    "\n",
    "    mean_list.append(np.mean(D))\n",
    "    max_list.append(np.max(D))\n",
    "    min_list.append(np.min(D))\n",
    "#     print('node k=%d - mean %f, max %f, min %f \\n' % (knn_k, np.mean(D), np.max(D), np.min(D)))\n",
    "    \n",
    "node_knn_mean = np.mean(np.asarray(mean_list))\n",
    "node_knn_min = np.mean(np.asarray(min_list))\n",
    "node_knn_max = np.mean(np.asarray(max_list))\n",
    "print('node k=%d - mean %f, max %f, min %f \\n' % (knn_k, node_knn_mean, node_knn_max, node_knn_min))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# how many points in kitti after radius cutoff\n",
    "def make_dataset_kitti(root):\n",
    "    seq_list = list(range(11))\n",
    "\n",
    "    # filter or not\n",
    "    np_folder = 'np_0.20_20480_r90_sn'\n",
    "        \n",
    "    accumulated_sample_num = 0\n",
    "    sample_num_list = []\n",
    "    accumulated_sample_num_list = []\n",
    "    folder_list = []\n",
    "    for seq in seq_list:\n",
    "        folder = os.path.join(root, 'data_odometry_velodyne', 'numpy', '%02d'%seq, np_folder)\n",
    "        folder_list.append(folder)\n",
    "        \n",
    "        sample_num = round(len(os.listdir(folder)))\n",
    "        accumulated_sample_num += sample_num\n",
    "        sample_num_list.append(sample_num)\n",
    "        accumulated_sample_num_list.append(round(accumulated_sample_num))\n",
    "        \n",
    "    return seq_list, folder_list, sample_num_list, accumulated_sample_num_list\n",
    "\n",
    "\n",
    "# statistics of Farthest Point Sampling, KITTI\n",
    "sampler = FarthestSampler()\n",
    "radius_cutoff = 50\n",
    "\n",
    "seq_list, folder_list, sample_num_list, accumulated_sample_num_list = make_dataset_kitti('/ssd/jiaxin/dataset/odometry')\n",
    "num_list = []\n",
    "for i in range(0, 20000, 100):\n",
    "\n",
    "    for idx, accumulated_sample_num in enumerate(accumulated_sample_num_list):\n",
    "        if i < accumulated_sample_num:\n",
    "            break\n",
    "    folder = folder_list[idx]\n",
    "    seq = seq_list[idx]\n",
    "\n",
    "    if idx == 0:\n",
    "        index_in_seq = i\n",
    "    else:\n",
    "        index_in_seq = i - accumulated_sample_num_list[idx-1]\n",
    "    pc_np_file = os.path.join(folder, '%06d.npy' % index_in_seq)\n",
    "    pc = np.load(pc_np_file)\n",
    "    \n",
    "\n",
    "    # point cloud z cutoff\n",
    "    valid_index = np.linalg.norm(pc[:, [0, 2]], axis=1) < radius_cutoff\n",
    "    pc = pc[valid_index, :]\n",
    "    \n",
    "    num_list.append(pc.shape[0])\n",
    "plt.hist(num_list)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for i in range(100):\n",
    "    pc, pose = load_pc_and_pose_kitti(0, i)\n",
    "    \n",
    "    reflectance = pc[:, 3]\n",
    "    print(np.mean(reflectance))\n",
    "    print(np.max(reflectance))\n",
    "    print(np.min(reflectance))\n",
    "    print('---')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(np.linalg.norm([1,1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# make Oxford dataset\n",
    "def make_dataset_oxford(txt_path):\n",
    "    f = open(txt_path, 'r')\n",
    "    lines_list = f.readlines()\n",
    "    \n",
    "    dataset = []\n",
    "    for i, line_str in enumerate(lines_list):\n",
    "        # convert each line to a dict\n",
    "        line_splitted_list = line_str.split('|')\n",
    "        try:\n",
    "            assert len(line_splitted_list) == 3\n",
    "        except Exception:\n",
    "            print('Invalid line.')\n",
    "            print(i)\n",
    "            print(line_splitted_list)\n",
    "            continue\n",
    "        \n",
    "        file_name = line_splitted_list[0].strip()\n",
    "        positive_lines = list(map(int, line_splitted_list[1].split()))\n",
    "        non_negative_lines = list(map(int, line_splitted_list[2].split()))\n",
    "        \n",
    "#         print(file_name)\n",
    "#         print(positive_lines)\n",
    "#         print(non_negative_lines)\n",
    "\n",
    "        data = {'file': file_name[0:-3]+'npy', 'pos_list': positive_lines, 'nonneg_list': non_negative_lines}\n",
    "        dataset.append(data)\n",
    "        \n",
    "    f.close()\n",
    "    return dataset"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# statistics of Farthest Point Sampling, Oxford\n",
    "sampler = FarthestSampler()\n",
    "radius_cutoff = 50\n",
    "k = 256\n",
    "\n",
    "train_folder_path = '/ssd/dataset/oxford/train_np'\n",
    "train_txt_path = '/ssd/dataset/oxford/train_relative.txt'\n",
    "dataset = make_dataset_oxford(train_txt_path)\n",
    "\n",
    "mean_list = []\n",
    "min_list = []\n",
    "max_list = []\n",
    "for i in range(0, 20000, 100):\n",
    "\n",
    "    pc_file = dataset[i]['file']\n",
    "    pc_np = np.load(os.path.join(train_folder_path, pc_file))\n",
    "\n",
    "    # point cloud z cutoff\n",
    "    valid_index = np.linalg.norm(pc[:, [0, 2]], axis=1) < radius_cutoff\n",
    "    pc = pc[valid_index, :]\n",
    "\n",
    "    # point cloud random sampling\n",
    "    pc = pc[np.random.choice(pc.shape[0], 16384, replace=False), :]\n",
    "\n",
    "    farthest_pts = sampler.sample(pc[:, 0:3], k)\n",
    "\n",
    "    # visualization\n",
    "#     fig = plt.figure(figsize=(9, 9))\n",
    "#     ax = Axes3D(fig)\n",
    "#     ax = plot_pc(pc, color=[0, 0, 1], ax=ax)\n",
    "#     ax = plot_pc(farthest_pts, color=[1, 0, 0], size=3, ax=ax)\n",
    "#     plt.show()\n",
    "\n",
    "    # nearest neighbor analysis for point cloud\n",
    "#     knn_k=2\n",
    "#     knn = KNNBuilder(k=knn_k)\n",
    "\n",
    "#     index = knn.build_nn_index(np.ascontiguousarray(pc[:, 0:3], dtype=np.float32))\n",
    "#     D, I = knn.search_nn(index, np.ascontiguousarray(pc[:, 0:3], dtype=np.float32), k=knn_k)\n",
    "\n",
    "#     D = np.sqrt(np.fabs(D))\n",
    "#     D = D[:, knn_k-1:knn_k]\n",
    "#     print('pc k=%d - mean %f, max %f, min %f' % (knn_k, np.mean(D), np.max(D), np.min(D)))\n",
    "    \n",
    "    \n",
    "    # nearest neighbor analysis for nodes\n",
    "    knn_k=2\n",
    "    knn = KNNBuilder(k=knn_k)\n",
    "\n",
    "    index = knn.build_nn_index(np.ascontiguousarray(farthest_pts[:, 0:3], dtype=np.float32))\n",
    "    D, I = knn.search_nn(index, np.ascontiguousarray(farthest_pts[:, 0:3], dtype=np.float32), k=knn_k)\n",
    "\n",
    "    D = np.sqrt(np.fabs(D))\n",
    "    D = D[:, knn_k-1:knn_k]\n",
    "    \n",
    "    mean_list.append(np.mean(D))\n",
    "    max_list.append(np.max(D))\n",
    "    min_list.append(np.min(D))\n",
    "#     print('node k=%d - mean %f, max %f, min %f \\n' % (knn_k, np.mean(D), np.max(D), np.min(D)))\n",
    "    \n",
    "\n",
    "node_knn_mean = np.mean(np.asarray(mean_list))\n",
    "node_knn_min = np.mean(np.asarray(min_list))\n",
    "node_knn_max = np.mean(np.asarray(max_list))\n",
    "print('node k=%d - mean %f, max %f, min %f \\n' % (knn_k, node_knn_mean, node_knn_max, node_knn_min))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
